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摘 要 : 针对 多 AUV(autonomous underwater vehicle) 系 统 在 未 知 环境 中 进行 路 径 规划 时 ， 难 以 兼顾 避 障 与 编队 的 问 
题 , 提出 了 一 种 基于 领航 -跟随 者 与 行为 的 多 AUV 协同 如 障 方法 。 首先 , 通过 构造 碰撞 危险 度 及 偏离 目标 评价 函数 ， 
设计 了 AUV 局 部 路 径 规 划 方 法 ; 在 此 基础 上 ， 结 合 编队 控制 方法 ， 分 别 为 领航 者 和 跟随 者 设计 不 同 的 行为 以 及 行 
为 选择 模式 。 半 物理 仿真 实验 结果 表明 ， 该 算法 能 够 实现 多 AUV 系统 在 未 知 环境 中 的 协同 避 障 ， 且 队 形 偏离 度 与 
恢复 队 形 时 间 优 于 传统 多 机 器 人 避 障 算法 。 实 验 结果 证 明了 该 算法 的 可 行 性 与 有 效 性 。 
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Research on multi-AUV cooperative obstacle avoidance method in unknown environment 


Liu Yal23, Zeng Junbao!i 
(1. State Key Laboratory of Robotics, Shenyang Institute of Automation, Chinese Academy of Sciences, Shenyang 110016, 
China; 2. Institutes for Robotics & Intelligent Manufacturing, Chinese Academy of Sciences, Shenyang 110169, China; 
3. University of Chinese Academy of Sciences, Beijing 100049, China) 


Abstract: Aiming at the problem that it is difficult to take into account both obstacle avoidance and formation when multi- 
AUYV system plans path in unknown environment, this paper proposed a multi-AUV cooperative obstacle avoidance method 
based on leader-follower and behavior. First, this paper designed a local path planning method for AUV by constructing the 
evaluation function of collision risk and deviation from the target; on this basis, combined with formation control method, 
this paper designed different behaviors and behavior selection patterns for leader and followers respectively. The semi- 
physical simulation experiment results Show that the algorithm can realize the cooperative obstacle avoidance of multi-AUV 
system in unknown environment, and the formation deviation and recovery time are better than traditional multi-robot obstacle 
avoidance algorithms. Experimental results verify the feasibility and effectiveness of the algorithm. 
Key words: path planning; multi-auv; cooperative obstacle avoidance; Leader-Follower; behavior 
0 引言 种 方法 虽然 将 多 机 器 人 避 障 简化 为 了 单机 器 人 的 避 障 问题 ， 
但 使 得 领航 者 路 径 规划 算法 的 复杂 度 提高 ， 并 且 在 严格 保持 
多 机 器 人 系统 相 较 于 单机 器 人 而 言 ， 具 有 灵活 性 好 、 探 索 。 队 形 的 情况 下 ,会 使 得 编队 整体 无 法 通过 狭窄 区 域 ， 文 献 [6] 
范围 广 、 鲁 棒 性 强 等 优点 ， 并 且 能 够 完成 单机 器 人 难以 完成 的 ”中 采用 了 基于 行为 的 编队 控制 方法 ， 为 每 个 机 器 人 设置 驶 向 
任务 。 AUV 是 探索 与 开发 海洋 的 重要 工具 ， 随 着 对 海洋 环境 下 目标 点 、 避 障 、 沿 墙 、 避 碰 以 及 编队 等 不 同行 为 ， 实 验证 明 
究 的 不 断 深入 , 多 AUV 系统 开始 逐渐 应 用 于 目标 搜 探 、 协同 定 。 编队 整体 能 够 顺利 通过 障碍 物 区域 。 但 每 个 机 器 人 通过 不 同 
位 等 领域 。 在 多 机 器 人 系统 中 ， 编 队 控制 作为 一 个 重要 问题 被 。 行为 的 加 权 进 行 决策 ， 使 得 队 形 不 易 保持 ; 文献 [7] 采 用 人 工 
广泛 研究 。 目 前 主要 的 编队 控 秆 势 场 法 实现 了 多 AUV 系统 的 实时 避 障 ， 但 编队 效果 不 佳 ; 
基于 行为 的 方法 中、 人 工 势 场 法 外 、 虚 拟 结构 法 外 等 。 除 此 之 外 ， 部 分 学 者 通过 将 不 同 的 编队 控制 方法 相 结合 ， 比 
在 多 机 器 人 系统 执行 任务 的 过 程 中 ， 由 于 缺少 对 环境 的 。” 如 将 人 工 势 场 法 与 一 致 性 算法 结合 后 ! 叶 ,领航 -跟随 者 与 人 工 
先 验 知识 ， 提 前 规划 好 的 路 径 上 可 能 会 出 现 障 碍 物 。 机 器 人  ” 势 场 法 结合 042， 领 航 - 跟 随 者 与 基于 行为 方法 结合 0319， 弥 
需要 实时 规划 路 径 以 躲避 障碍 物 ， 同 时 还 要 避免 因 路 线 发 生 ” 补 单一 方法 的 缺陷 ， 提 升 了 多 机 器 人 避 障 的 效果 。 
改变 而 与 其 他 机 器 人 碰撞 。 除 此 之 外 ， 机 器 人 之 间 的 距离 不 考虑 到 障碍 物 环 境 的 复杂 与 多 样 性 以 及 机 器 人 的 能 耗 问 
能 过 远 ， 和 否则 可 能 会 影响 机 器 人 之 间 的 通信 ， 甚 至 在 避 障 结 题 ， 一 般 不 将 多 机 器 人 系统 看 做 刚性 结构 。 在 这 个 前 提 下 ， 
束 后 无 法 恢复 编队 。 因 此 ， 为 了 保证 机 器 人 的 安全 以 及 后 颖 于 每 个 机 器 人 需要 同时 考虑 编队 约束 以 及 障碍 物 约束 ， 多 
任务 的 正常 执行 ， 机 器 人 之 间 需 要 通过 相互 协作 ， 在 躲避 障 。 机 器 人 系统 难以 保持 原 有 队 形 ， 需 要 局 部 调整 位 置 进行 局 部 
但 物 的 同时 兼顾 编队 需求 。 由 于 系统 中 的 每 个 机 器 人 受到 障 。” 队 形变 换 ， 或 者 根据 障碍 物 环境 选择 合适 的 队 形 进行 整体 队 
碍 物 位 置 以 及 队 形 的 多 方面 约束 ， 使 得 多 机 器 人 协同 避 障 问 ” 形变 换 。 目 前 对 于 整体 队 形变 换 的 研究 包括 障碍 物 约束 下 队 
题 更 为 困难 。 形 选择 方法 03、 队 形变 换 的 策略 0419。 文 献 [18] 中 引入 障碍 
目前 ， 很 多 学 者 在 编队 控制 方法 的 基础 上 ， 对 多 机 器 人  ” 物 密度 的 概念 ， 机 器 人 通过 感知 到 的 障碍 物 密度 局 部 调整 位 
协同 避 障 问题 展开 了 研究 。 文 献 [5] 在 领航 -跟随 者 队 形 结构 。” 置 ,这 种 方法 使 多 机 器 人 系统 呈现 出 自 适 应 队 形 变换 的 特点 ， 
的 基础 上 ， 将 编队 整体 看 做 刚性 结构 ， 通 过 为 领航 者 进行 路 但 障碍 物 密度 这 一 指标 实际 上 难以 计算 。 
径 规 划 以 及 设置 队 形 转向 实现 障碍 物 环境 下 的 编队 控制 。 这 基于 以 上 分 析 ， 本 文 将 两 种 编队 控制 方法 相 结 合 ， 提 日 
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刘 亚 ， 等 : 未 知 环境 下 多 AUV 协同 避 障 方法 研究 


` 境 下 基于 领航 -跟随 者 与 行为 的 多 AUV 协同 避 障 


方法 。 领 航 者 负责 队 形 选择 以 及 规划 编队 整体 前 进 路 线 ， 每 


台 AUYV 通 


过 行为 的 选择 进行 决策 , 使 多 AUYV 系统 在 避 障 的 


同时 能 够 自 适 应 调整 队 形 ， 并 且 根据 环境 的 需要 变换 合适 的 


队 形 。 仿 真实 验证 明了 水 下 不 同 场景 下 算法 的 有 效 性 。 
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基于 前 视 声 纳 的 AUV 局 部 路 径 规划 方法 


机 器 人 的 路 径 规划 一 般 分 为 环境 建 模 与 路 径 搜索 两 部 分 


09。 目 前 主要 的 环境 建 模 方法 包括 栅 格 法 、 几 何 表示 法 以 及 
拓扑 表示 法 等 0， 本 文采 用 几何 学 表示 的 方法 。 同 时 ， 考 虑 


到 多 边 形 相 较 于 风 
障碍 物 轮廓 和 


如 图 


13 13x(i—1) 


为 [二 z+ ,一 


9 最 小 凸 多 边 形 进行 环境 建 模 。 
1 所 示 , AUV 使 用 前 视 声 纳 对 环境 进行 感知 , 返 
的 声 纳 图 像 中 包含 探测 到 的 前 方 环境 信息 。 
视 声 纳 水 平 开 


人 
为 130 度 , 作用 距离 L=100m 将 前 视 声 纳 的 
探测 区 


或 平均 分 成 N 部 分 , 则 每 部 分 8%G=12…N) 的 探测 


可 拟 合 的 轮廓 形状 更 为 广泛 ， 因 此 采用 


实验 中 采用 上 


13 13xi 


36 18N 


下 二 o 
36 -18N 


规定 划分 的 每 部 分 探测 区 域 的 中 间 航 向 G=L12…N) 为 


此 部 分 的 代表 航向 ， 通 过 建立 评价 函数 对 内 进行 评估 ， 以 确 
定 AUV 下 一 时 刻 的 航行 方向 。 


点 航行 ， 当 存在 障碍 物 时 ，AUYV 需要 改变 航向 以 避免 碰撞 。 
某 一 航向 上 AUYV 与 障碍 物 的 距离 越 近 ， 说 明 沿 着 该 航向 航 
行 碰撞 危险 性 
为 与 目标 点 航向 的 偏 


Fig. 1 
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图 1 


前 视 声 纳 模型 


Forward looking sonar model 


当 目 标点 航向 上 不 存在 障碍 物 时 , AUV 会 直接 朝 着 目标 


越 高 。 


此 ， 评 价 函数 包含 两 部 分 : 第 一 部 分 


离 程度 ， 第 二 部 分 为 与 障碍 物 碰撞 的 危 


险 程度 。 
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别 进行 归 一 化 处 理 : 


Goal(y)=180—|w; —Wsoal (1) 
Dist(w,) = D D,<D. 2 
YY D>D O) 


中 Wo 为 目标 点 航向 ，D 为 y 方 向 上 与 障碍 物 的 距离 ，D, 
设置 的 距离 闵 值 。 
在 计算 总 评价 函数 之 前 ， 需 要 对 评价 函数 中 的 两 部 分 分 


Goal,(y) = 


DGoal(y,) G3) 


Dist, (yi) = 


DDist(y,) (4) 


根据 下 式 对 待 评估 航向 进行 评分 ， 分 值 最 高 的 航向 为 当 


前 时 刻 的 决策 航向 : 
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2.1 
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Ip 


Evaluate(y;) = & :Goal, (y;)+ PB: Dist,(y) 


(5) 


多 AUV 协同 避 障 方法 


基于 领航 -跟随 者 的 编队 控制 方法 
领航 -跟随 者 是 一 种 常见 的 编队 控制 方法 ， 主 要 包括 /-/ 


出 及 1 一 9 控制 1。 


本 文采 用 1-9 控制 方法 ， 跟 随 者 需要 与 
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领航 者 保持 一 定 的 距离 和 角度 。 
AUYV 的 位 姿 采 用 (x,y,w) 表示 , 其 中 (x,y) 为 AUV 在 固定 
坐标 系 下 的 坐标 ，¥ 代表 AUV 的 航向 角 。 
如 图 2 所 示 ， 领 航 者 的 位 姿 为 ,yi,wWi) ， 根 据 领 航 者 信 


息 及 队 形 参数 可 以 计算 出 跟随 者 的 期 望 位 姿 : 
Xslt) | fxr(t) +licos(g; +y(t)) 
| Dy o| =| yi(D +lisin(g, + Wr | (0) 
Wr(D) Wiz(D) 
Ya 
pA 
y2(n) 区 

rai 
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1 
X(t) 坊 
领航 -跟随 者 模型 


图 2 
Fig.2 Leader-follower model 
2.2 多 AUV 协同 避 障 控制 架构 
如 图 3 所 示 ， 本 文 在 领航 -跟随 者 结构 的 基础 上 , 结合 基 


于 行为 的 编队 控制 方法 ， 分 别 为 领航 者 和 跟随 者 设置 不 同 的 
行为 。 领 航 者 的 目标 点 为 多 AUV 系统 避 障 最 终 目标 点 ， 跟 
随 者 的 目标 点 为 满足 期 望 队 形 的 虚拟 目标 点 。 每 一 时 刻 领航 
者 通过 环境 感知 及 避 障 目标 点 进行 行为 选择 与 决策 ， 规 划 编 
队 整 体 前 进 路 线 ， 同 时 进行 队 形 选择 ， 并 将 当前 位 姿 与 队 形 
参数 发 送 给 跟随 者 ， 跟 随 者 实时 计算 虚拟 目标 点 ， 并 结合 


口 口 


身 的 感知 信息 进行 行为 选择 与 决策 。 
感知 信息 中 队 形 选择 
目标 点 Ee : 
| 行为 1 | 
| 2 形 9 本 
| od 行为 2 ! y 
一 | 一 > 控制 器 | 对 执行 器 
虚拟 目标 点 Wa | 
| 行为 n 
感知 信息 
跟随 者 行为 选择 
图 3 多 AUYV 协同 避 障 控制 架构 


Fig.3 Multi-AUV cooperative obstacle avoidance control architecture 
2.3 领航 者 行为 设置 与 队 形 选择 
根据 提出 的 多 AUV 协同 避 障 控制 架构 ， 本 节 分 别 对 领 


航 者 的 行为 设置 、 行 为 选择 及 队 形 选择 方法 进行 说 明 。 

为 保证 AUYV 的 平稳 运行 ， 规 定 领航 者 在 避 障 过 程 中 的 
速度 大 小 4 保持 不 变 。 为 领航 者 设置 驶 向 目标 点 以 及 避 障 两 
种 行为 。 设 领航 者 的 决策 速度 为 凡 ， 

Ve ei 
vl nl | 0 


中 7 为 行为 选择 矩阵 , 每 一 时 刻 其 中 的 元 素 只 有 一 个 为 1， 
余 为 0。Vrwe_o_wn 与 Wo wma 分 别 为 驶 向 目标 点 行为 和 避 障 
J 为 的 决策 速度 。 

1) 驶 向 目标 点 行为 

驶 向 目标 点 行为 的 决策 速度 可 以 表示 为 


二 
也 = < | 人 | 
P(X,X) ye-y 
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(8) 
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其 中 cx) 为 目标 点 坐标 ，Ccy) 为 当前 位 置 坐 标 ，P(X.X,) 为 
当前 位 置 与 目标 点 的 欧式 距离 。 


2) 避 障 行为 

领航 者 的 避 障 方法 已 在 本 文 第 1 章 中 详细 阅 述 。 为 避免 
在 避 障 过 程 中 跟随 者 之 间 发 生 磁 撞 ， 增 大 领航 者 与 障碍 物 之 
间 的 安全 距离 4 。 设 跟随 者 与 障碍 物 之 间 的 安全 距离 为 4 ,中 
随 者 之 间 允 许 的 最 小 距离 为 ko ， 则 设置 


d 

di=d+—= 
+ 全 9) 
避 障 行为 的 决策 可 以 表示 为 
To _obstacle 二 下 | 


其 中 为 局 部 路 径 规 划 方 法 的 决策 航向 。 
3) 行为 选择 
领航 者 行为 选择 方法 如 下 : 

a) 判 断 前 视 声 纳 视野 内 是 否 存在 障碍 物 ， 若 不 存在 ， 执 

行驶 向 目标 点 行为 ; 否则， 转向 步骤 b); 

b) 计 算 目 标点 航向 与 障碍 物 的 距离 p ， 若 小 于 所 设 定 的 
距离 阔 值 D,， 执 行 避 障 行为 ， 否 则 ， 执 行驶 向 目标 点 行为 。 
4) 队 形 选择 
共 设 置 三 角形 ( 原 队 形 ) 以 及 一 字形 两 种 队 形 。 若 领航 者 

的 决策 航向 在 当前 位 置 与 两 障碍 物 切线 之 间 ， 认 为 领航 者 决 

定 从 两 障碍 物 之 间 通 行 (图 4), 此 时 计算 两 个 障碍 物 之 间 的 最 

近 距 离 Da 。 若 Dam > dwm， 选择 三 角形 队 形 ; 否则 选择 一 字 队 

形 。 


el (10) 


siny 


Di, = mindist {obstaclet obstacle,} (11) 


障碍 物 1 障碍 物 2 


| 领航 者 


图 4 ”从 两 障碍 物 之 间 通 行 
Fig.4 Passthrough between two obstacles 
2.4 跟随 者 行为 设置 与 行为 选择 
根据 提出 的 多 AUV 协同 避 障 控制 架构 ， 跟 随 者 需 根 据 
领航 者 发 送 的 位 姿 信息 、 队 形 参数 计算 当前 时 刻 的 虚拟 目标 
点 ， 并 结合 感知 信息 进行 行为 的 选择 与 决策 。 因 此 ， 本 节 对 
跟随 者 的 行为 设置 及 行为 选择 进行 说 明 。 
为 跟随 者 设置 四 种 不 同 的 行为 ,分 别 为 驶 向 目标 点 行为 、 
避 障 行为 、 保 持 行 为 以 及 跟随 行为 。 不 同 于 领航 者 ， 跟 随 者 
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2) 避 障 行为 

跟随 者 的 避 障 方法 已 在 本 文 第 1 章 中 详细 阐述 。 当 跟随 
者 执行 避 障 行为 时 ， 由 于 偏离 了 虚拟 目标 点 方向 ， 因 此 无 法 
保持 编队 队 形 。 这 种 情况 下 ， 出 于 以 下 两 点 的 考虑 ， 跟 随 者 
与 领航 者 之 间 保 持 距离 不 变 , 只 改变 角度 是 较为 理想 的 情况 : 
a) 保 证 领航 者 与 跟随 者 的 通信 距离 ，b) 保 证 跟随 者 与 领航 者 
之 间 不 会 发 生 碰 撞 。 
如 图 5 所 示 ， 建 立 以 领航 者 为 中 心 ， 半 径 为 ! 的 圆 ，! 为 


~ 
2 


跟随 者 与 领航 者 的 编队 距离 。 当 决策 航向 所 在 的 直线 与 区 
交点 时 ， 与 跟随 者 距离 较 近 的 交点 为 跟随 者 的 当前 目标 ; 
决策 航向 所 在 的 直线 与 圆 无 交点 时 ,扩大 圆 的 半径 直至 相 切 ， 
切 点 为 跟随 者 的 当前 目标 。 


sa 


wa 


(a) 与 圆 有 交点 〈 外 部 ) (b) 与 圆 有 交点 《内 部 ) 


(G) 与 圆 无 交点 
图 5 避 障 行为 当前 目标 点 的 计算 


Fig.5 Calculation of current target for obstacle avoidance behavior 
避 障 行为 的 决策 可 以 表示 为 


cosy 
(utep(X,X.)| |a(X,X.)-w 0 
siny 
(14) 


Vowia _obstacle 一 


cosy 
aptx,X | | [a(X,X.)-v lx 
siny 


其 中 p(X,X,) 为 当前 位 置 与 当前 目标 点 的 欧式 距离 ，w 为 局 
部 路 径 规 划算 法 决策 航向 ,，a(X,X.) 表示 从 当前 位 置 指向 当前 
目标 点 的 射线 与 x 轴 的 夹 角 。 

3) 保持 行为 

在 1-9 的 编队 控制 结构 下 , 当 领 航 者 航向 角 发 生 改 变 时 ， 
跟随 者 的 虚拟 目标 点 位 置 会 发 生 突变 。 对 位 于 领航 者 转弯 内 侧 
的 跟随 者 来 说 ， 目 标点 可 能 会 位 于 其 后 方 。 此 时 使 跟随 者 转弯 
以 跟随 后 方 目标 点 显然 是 不 合理 的 ， 保 持 行 为 设置 其 维持 当前 


编队 以 及 队 形变 换 需求 ， 因 此 在 避 障 过 程 中 速度 大 小 发 
生 改 变 。 设 跟随 者 的 决策 速度 为 Vi， 


void _obstacle 12 
Vi (12) 


其 中 : J 为 行为 选择 矩阵 ， Vw wo ， 
分 别 对 应 四 种 行为 的 决策 速度 。 
1) 驶 向 目标 点 行为 


到 


驶 向 目标 点 行为 的 决策 可 以 表示 为 
ut+ép(X,X,,)| xs —X 
Vm 
moe ten = (KK) kal (13) 
其 中 G63yw) 为 虚拟 目标 点 坐标 ， (xy) 为 当前 位 置 坐标 ， 


P(X,X,) 为 当前 位 置 与 虚拟 目标 点 的 欧式 距离 ,* 为 领航 者 速 
度 大 小 ，。 为 速度 增益 。 


航向 ， 并 减速 等 待 目 标点 。 保 持 行 为 的 决策 可 以 表示 为 
COsW, 

We -reo0xa 咱 | (15) 
siny. 


其 中 p(X,X) 为 当前 位 置 与 虚拟 目标 点 的 欧式 距离 ，yw. 为 
AUYV 当前 航向 。 

4) 跟随 行为 
当 领 航 者 发 布 队 形 为 一 字 队 形 时 ， 说 明 前 方 可 通行 的 宽 
度 较 小 。 此 时 ， 在 形成 编队 后 ， 跟 随 者 直接 跟踪 领航 者 的 轨 
迹 ， 即 跟随 者 将 领航 者 发 送 的 位 置信 息 保存 ， 作 为 自己 的 
标点 序列 。 

5) 行为 选择 

跟随 者 行为 选择 如 下 : 

a) 若 当前 队 形 为 一 字形 且 已 经 形成 编队 , 执行 跟随 行为 ; 
否则 转向 步 又 b); 
b) 判 断 虚 拟 目 标点 位 于 当前 位 置 的 前 方 或 后 方 ， 若 为 前 
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方 ， 转 向 步骤 c];， 否则 转向 步骤 d); 

9 计算 目标 航向 与 障碍 物 的 最 近 距 离 2 ， 若 小 于 设置 的 
阔 值 D, ， 执 行 避 障 行为 ,否则 执行 驶 向 目标 点 行为 ; 
d) 计 算 当 前 航向 与 障碍 物 的 最 近 距 离 2 ， 若 小 于 设置 的 
闵 值 D; , 执行 避 障 行为 (当前 航向 作为 避 障 算法 的 目标 航向 ); 
否则 执行 保持 行为 。 
需要 说 明 的 是 ， 在 领航 者 发 布 变 换 队 形 的 指令 后 ， 为 了 
快速 形成 编队 ， 跟 随 者 的 行为 选择 方式 有 所 不 同 。 此 时 ， 若 
当前 位 置 与 目标 点 之 间 无 障碍 物 ， 选 择 驶 向 目标 点 行为 ， 待 
新 的 编队 形成 后 ， 恢 复 上 述 的 行为 选择 方式 。 


3 ”仿真 实验 


为 证 明 算 法 的 可 行 性 ， 本 文采 用 视 景 平台 以 及 真实 的 水 
下 机 器 人 控制 软件 进行 了 仿真 实验 ， 包 括 单 台 AUV 的 避 障 实 
验 以 及 三 台 AUV 协同 避 障 实验 。 如 图 6、7 所 示 ， 视 景 平台 在 
实际 的 多 波束 测 深 数据 、 海 图 数据 等 的 基础 上 构建 了 逼真 的 三 
维 场景 ， 可 以 设置 多 台 AUV 并 接收 显示 AUV 的 实时 位 姿 。 同 
时 , 视 景 平台 还 可 以 设置 前 视 声 纳 的 作用 距离 , 并 根据 AUYV 在 
前 视 声 纳 探测 范围 内 不 同 角 度 距 障碍 物 的 距离 实时 模拟 并 返 下 


前 视 声 纳 图 像 ， 这 与 真实 前 视 声 纳 的 成 像 原理 类 似 。 平 台 可 以 
设置 不 规则 形状 、 长 方 体 、 圆 柱 体 三 种 不 同类 型 的 障碍 物 。 水 
下 机 器 人 控制 软件 能 够 按照 一 定 的 频率 模拟 AUV 之 间 的 通信 。 
实验 中 每 台 AUV 均 在 100m 深度 定 深 航行 。 


六 


图 6 视 景 平台 


Fig.6 Viewing platform 


图 7 前 视 声 纳 图 像 


Fig.7 Forward looking sonar image 
每 台 AUV 的 仿真 框架 如 图 8 所 示 : 在 视 景 平台 上 设 
障碍 物 ， 平 台 会 回 传 每 个 AUV 的 前 视 声 纳 信息 ， 每 个 AUV 
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障 过 程 中 航向 总 变化 量 计算 。 实 验 结果 显示 ， 在 不 同 场景 下 
AUYV 均 顺 利通 过 障碍 物 区 域 , 并 在 避 障 过 程 中 与 障碍 物 保持 
足够 的 安全 距离 。 


本 文 算法 


目标 点 | 速度 PID 


设置 障碍 物 
ee 控制 器 


航向 PID 
控制 器 


图 8 AUYV 仿真 框架 
Fig.8 _ Simulation framework of AUV 


50 


50 


二 避 障 航线 ] 二 各 感 航 线 | | 
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@ 
-50| @ -50 
电 日 
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-150 -150 
250 SN 
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350 
700| | 一 加 障 航 线 
- - 原 定 航线 
250 
S00 
皇 150 8 
> > 300 
30 100 
-50 1 -100 
-100 0 100 200 -100 100 300 S00 700 
X/(m) X/(m) 
(0) 复 杂 场 景 (qd) 海 山 场景 


9 本 文 算法 在 不 同 场景 下 规划 结果 
Fig.9 Planning results ofthe proposed algorithm in 
different scenarios 
表 1 不 同 场景 下 避 障 性 能 指标 统计 
Tab. 1 Statistics of obstacle avoidance performance 

indicators in different scenarios 


场景 最 小 距离 /m 运 行 时 间 /s 路 径 长 度 /m 路 径 平 滑 度 /(”) 


离散 障碍 物 10.15 289.68 430.37 108.34 
不 同类 型 障碍 物 。” 10.21 314.72 459.56 350.25 
复杂 场景 7.73 286.67 418.50 322.73 
海山 场景 10.42 922.17 1374.82 257.18 


为 进一步 验证 本 文 算 法 的 有 效 性 ， 在 离散 障碍 物 场景 下 
采用 人 工 势 场 法 及 RRT 算法 进行 规划 , 并 与 本 文 算法 规划 结 
果 进 行 对 比 ， 结 果 如 图 10、 表 2 所 示 。 实 验 结果 表明 ， 在 安 
全 距离 相当 的 情况 下 ， 本 文 算法 相 较 于 人 工 势 场 法 路 径 更 为 


根据 感知 信息 以 及 目标 点 按照 本 文 算 法 进行 决策 ， 得 到 速度 
指令 了 ; 分 别 将 速度 大 小 及 速度 方向 (航向 ) 传 给 对 应 的 PID 
控制 器 , 得 到 相应 的 控制 量 ; 将 控制 量 传 入 AUYV 模型 ， 从 而 
得 到 更 新 后 的 位 姿 ; 最 后 将 位 姿 传 给 视 景 平台 进行 实时 更 新 
显示 。 这 种 仿真 方式 从 AUV 的 环境 感知 、 运 动 控 制 及 通信 
等 方面 较 大 程度 地 模拟 了 真实 AUV 的 情况 。 
3.1 单 AUV 避 障 仿真 实验 

为 验证 本 文 AUV 局 部 路 径 规划 算法 , 设置 单 AUV 避 障 


Hl 


仿真 实验 ,实验 中 AUV 的 速度 大 小 保持 不 变 , 设置 w=1.5 m/s。 


分 别 在 离散 障碍 物 场景 、 不 同类 型 障碍 物 场 景 、 复 杂 场 景 以 
及 海山 场景 中 采用 本 文 算法 进行 实验 , 规划 结果 如 图 9、 表 1 
所 示 。 以 距 障 碍 物 的 最 小 距离 、 运 行 时 间 、 路 径 长 度 以 及 路 
径 平 滑 度 作 为 衡量 规划 结果 的 指标 ， 其 中 路 径 平 滑 度 使 用 避 


| 


F 滑 ， 运 行 时 间 更 短 。RRT 算法 由 于 具有 较 大 的 随机 性 ， 规 
划 的 路 径 较 为 曲折 。 


X/(m) X/(m) 


(a) 人 工 势 场 法 (b)RRT 
图 10 ”人工 势 场 法 及 RRT 算法 规划 结果 
Fig. 10 Planning results of artificial potential field method and RRT 
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表 2 不 同 算法 性 能 指标 对 比 
Tab.2 Comparison ofperformance indicators of different algorithms 
算法 ”最 小 距离 /m 运行 时 间 /s 路 径 长 度 /m 路 径 平 滑 度 / 


本 文 算法 10.15 289.68 430.37 108.34 
人 工 势 场 法 9.93 356.48 440 2283.82 
RRT 5.32 379.07 461.73 1666.57 


3.2 多 AUV 协同 避 障 仿真 实验 
为 验证 本 文 提 出 的 多 AUV 协同 避 障 算法 ， 采 用 三 台 
AUYV 进行 仿真 实验 。 算 法 参数 选取 如 表 3 所 示 。 
表 3 算法 参数 选取 
Tab. 3 Selection of algorithm parameters 
参数 ”安全 距离 /m 。 避 障 距 离 阔 值 /m ”速度 区 间 /(m/s) & pp & 
领航 者 15 80 1.5-1.5 0.60.4 / 
跟随 者 10 80 0-3.0 0.60.40.1 
如 图 11 所 示 ， 实 验 中 三 台 AUYV 的 期 望 队 形 为 三 角形 队 
形 ， 变 换 队 形 为 一 字 队 形 。 队 形 参数 的 选取 如 表 4 所 示 。 


7 


上 7 
Pr pr! 
9 学 1 
KR 
\ | ! 1 
多 AN 1 2 
让 | 
3 ， 
(a) 三 角形 (b) 一 字形 


图 11 三 角形 及 一 字 队 形 
Triangle formation and inline formation 
表 4 队 形 参数 选取 


Tab.4 Selection of formation parameters 


Fig. 11 
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多 AUV 系统 能 够 根据 障碍 物 情 况 自主 压缩 队 形 ， 呈 现 出 自 
适应 队 形变 换 的 特点 ， 且 AUV 之 间 能 够 保持 一 定 的 距离 ， 
保证 AUV 的 安全 。 多 AUYV 系统 在 避 障 结束 后 能 够 快速 恢复 
原始 队 形 ， 继 续 在 原 定 航线 上 执行 任务 。 
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13 ”简单 场景 多 AUV 协同 避 障 轨迹 


Fig. 13 ”Collaborative obstacle avoidance trajectories of 


multi-AUV in simple scenario 
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图 14 复杂 场景 多 AUYV 协同 避 障 轨迹 


Fig. 14 Collaborative obstacle avoidance trajectories of 


multi-AUV in complex scenario 


表 5 不 同 场景 下 协同 避 障 指标 统计 


三 角形 一 字形 Tab. 5 Statistics of collaborative obstacle avoidance performance 
oA a 及 1 indicators in different scenarios 
30m 150° 15m 30m 180° 场景 平均 队 形 偏离 度 /m 恢复 队 形 时 间 /s 
3.2.1 算法 仿真 离散 障碍 物 场景 9.86 4.01 
根据 上 述 的 算法 参数 及 队 形 参数 设置 ， 在 三 种 不 同 的 场 简单 场景 8.32 3.02 
景 下 进行 仿 复杂 场景 9.14 3.02 


实验 ， 分 别 为 离散 障碍 物 场景 、 简 单 场景 以 及 
复杂 场景 ,场景 设置 及 实验 结果 分 别 如 图 12~14、 表 5 所 示 。 
采用 平均 队 形 偏离 度 及 恢复 队 形 时 间作 为 衡量 规划 结果 的 指 
标 。 其 中 队 形 偏离 度 5 由 当前 队 形 与 期 望 队 形 计算 确定 : 


6=> ld 一 咱 (16) 
其 中 di 为 当前 时 刻 AUV 间距 离 , /为 期 望 队 形 AUV 间距 离 。 


恢复 队 形 时 间 定 义 为 从 最 后 一 台 AUYV 避 障 结束 开始 ， 到 队 
形 偏离 度 小 于 7.5 所 用 时 间 。 
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图 12 离散 障碍 物 场 景 多 AUYV 协同 避 障 轨迹 


Fig. 12 Collaborative obstacle avoidance trajectories of 


multi-AUV in discrete obstacle scenario 

出 ， 领 航 者 规划 编队 整体 前 进 路 线 ， 
跟随 者 通过 与 领航 者 进行 信息 交互 及 环境 感知 躲避 障碍 物 ， 
从 而 在 避 障 过 程 中 兼顾 编队 需求 。 从 队 形变 化 中 可 以 看 出 ， 


为 验证 算法 提出 的 队 形变 换 以 及 跟随 行为 ， 设 置 狭窄 水 
道场 景 , 五 部 分 的 宽度 分 别 为 60m、40m、28m、23m、28m， 
多 AUV 协同 避 障 实验 结果 如 图 15、16 所 示 。 
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图 15 ”狭窄 水 道场 景 多 AUV 协同 避 障 轨迹 
Fig. 15 Collaborative obstacle avoidance trajectories of 
multi-AUV in narrow waterway scenario 
实验 结果 显示 ， 多 AUV 通过 狭窄 水 道 可 分 为 以 下 几 个 
阶段 : 
a) 在 60m 宽 的 水 道中 ,编队 保持 原 队 形 即 可 安全 通过 ， 
当 跟 随 者 发 现 航行 前 方 存在 障碍 物 且 距离 小 于 设 定 的 阔 值 时 ， 
- 始 躲 避 障 得 物 ; 
b) 在 40m 宽 的 水 道中 , 两 个 跟随 者 均 需 要 避 障 , 队 形 宽 
度 压 缩 。 当 领航 者 计算 前 方 可 通行 宽度 小 于 编队 可 压缩 最 小 
宽度 时 ， 发 布 变换 一 字 队 形 命 令 ， 并 将 自身 与 障碍 物 之 间 的 
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安全 距离 调整 为 10m， 700 二 700 — 
c) 当 一 字 队 形 形 成 后 ， 跟 随 者 执行 跟随 行为 ， 将 领航 者 | IE 
的 轨迹 点 作为 自己 的 目标 点 ， 进 行 轨迹 跟踪 ; 区 
d) 当 领 航 者 声 纳 视野 内 不 存在 障碍 物 时 , 开始 驶 向 避 障 500 500 
目标 点 。 考 虑 到 跟随 者 的 安全 ， 领 航 者 在 40s 后 发 布 恢复 队 
形 的 命令 ， 跟 随 者 变换 为 三 角形 队 形 ; | Em 
e) 到 达 避 障 目标 点 后 ， 多 AUV 继续 在 原 定 航线 上 编队 六 300| > 00 
执行 任务 。 
200 200 上 
100 100 - 
90 100 200 300 % 100 200 300 
X/(m) X/(m) 
(a) 本 文 算法 (b) 编 队 拆 分 


寻 18 ”本 文 算法 与 编队 拆 分 避 障 实验 结果 
Fig. 18 Results of the proposed algorithm and formation split 
表 7 本 文 算法 与 编队 拆 分 实验 结果 对 比 


Tab.7 Comparison of results of the proposed 


图 16 跟随 者 执行 跟随 行为 algorithm and formation split 
Fig. 16 Followers perform follow behavior 算法 ”AUYV 间 最 近 距 离 /m AUYV 间 最 远 距离 /m 恢复 队 形 时 间 /s 
3.2.2 对 比 实验 本 文 算法 17.34 38.69 12.03 
为 进一步 验证 本 文 提出 的 多 AUV 协同 避 障 算法 的 有 效 。 ”编队 拆 分 27.71 162.08 39.17 
性 ， 将 其 与 目前 常用 于 障碍 物 环境 下 编队 控制 的 人 工 势 场 法 3.2.3 算法 可 扩展 性 实验 
进行 对 比 ， 结 果 如 图 17 所 示 。 上 述 实 验 对 不 同 场 景 下 三 台 AUV 协同 避 障 进行 了 算法 
加 8 | A 一 王族 验证 , 为 验证 算法 的 可 扩展 性 , 将 AUV 数量 增加 至 五 台 , 分 
| | 一 2 | 别 在 简单 场景 及 复杂 场景 中 进行 实验 。 五 台 AUV 的 编队 队 
, 形 及 队 形 参数 分 别 如 图 19、 表 8 所 示 。 
1 | + 
aw Bm | 
-350 350 
-450 | -450 
0 1 0 50 1350 250 350 450 350 
X/(m) X/(m) 
(a) 本 文 算法 (b) 人 工 势 场 法 图 19 五 台 AUYV 队 形 设置 
图 17 海山 场景 本 文 算法 与 人 工 势 场 法 实验 结果 Fig. 19 Setting of formation of five auvs 
Fig. 17 Results of the proposed algorithm and artificial potential 表 8 五 台 AUV 队 形 参数 
field method in seamount scenario Tab.8 Formation parameters of five auvs 
表 6 显示 ， 在 多 AUV 避 障 过 程 中 ， 本 文 算法 相 较 于 人 参数 数值 
工 势 场 法 具有 更 小 的 平均 队 形 偏离 度 ， 并 且 在 避 障 结束 后 能 li/m 30 
够 更 快速 恢复 队 形 ， 有 利于 多 AUYV 系统 后 续 任务 的 执行 。 b/m 60 
表 6 海山 场景 本 文 算法 与 人 工 势 场 法 实验 结果 对 比 2 人 ) 二 


算法 参数 的 选取 与 三 台 AUV 系统 相同 ， 实 验 结果 如 图 
20、21 所 示 。 实 验 结果 显示 ， 本 文 算法 在 不 同 场景 下 应 用 了 


Tab.6 Comparison ofresults ofthe proposed algorithm and 


nm 


artificial potential field method in seamount scenario 


算法 平均 队 形 偏离 度 /m 恢复 队 形 时 间 /s 五 台 AUV 系统 均 取得 了 良好 的 效果 。 
本 文 算法 13.38 8.04 2 ER 
一 3 号 跟随 者 
人 工 势 场 法 21.67 26.11 ' i ine 
在 目前 的 研究 中 ， 一 部 分 学 者 认为 当 多 机 器 人 系统 遇 到 -100 


， 避 障 的 优先 级 相 较 于 编队 更 高 ， 多 机 器 人 系统 应 


障碍 物 时 _ 
基于 此 ， 将 本 文 算 § 


当 解 除 编队 ， 在 躲避 障碍 物 后 恢复 队 
法 与 编队 拆 分 避 障 方法 进行 对 比 ， 结 果 如 图 18、 表 7 所 示 。 
于 编队 拆 分 避 障 不 具有 队 形 偏离 度 的 概念 , 采用 AUV 
间距 离 进行 对 比 。 在 实验 场景 中 ， 采 用 编队 拆 分 避 障 方法 
AUV 各 自 进 行 避 障 ， 选 择 从 障 但 物 两 侧 绕 行 ， 导 臻 AUV 昌 
离 过 远 ， 甚 至 造成 失 联 。 由 于 AUV 距离 过 远 , 避 障 结束 后 要 
花费 较 长 时 间 恢 复 队 形 。 相 较 于 编队 拆 分 避 障 ， 本 文 算法 在 
保证 AUV 之 间 保 持 一 定 距 离 的 前 提 下 , 将 AUV 之 间 的 最 远 Fig. 20 
距离 维持 在 编队 距离 附近 ,能够 在 避 障 结束 后 快速 恢复 编队 。 
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和 20 ”简单 场景 五 人 台 AUV 协同 避 障 轨迹 
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图 21 复杂 场景 五 台 AUV 协同 避 障 轨迹 
Fig.21 Collaborative obstacle avoidance trajectories of 
five auvs in complex scenario 
1 五 
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本 文 基于 前 视 声 纳 提出 了 一 种 AUV 局 部 路 径 规 划 方 法 ， 
并 在 此 基础 上 ， 结 合 编队 控制 方法 ， 提 出 了 一 种 未 知 环境 下 
多 AUV 协同 避 障 方法 。 行 为 的 设置 不 仅 考 虑 了 AUYV 与 障碍 
物 的 碰撞 ， 还 考虑 了 AUYV 之 间 的 相互 磁 撞 。 设 计 了 避 障 行 
为 下 目标 点 的 计算 方法 ， 使 跟随 者 在 避 障 过 程 中 与 领航 者 的 
距离 基本 维持 在 编队 距离 ， 保 证 AUYV 的 通信 与 安全 。 水 下 
多 种 场景 的 仿真 实验 均 取 得 了 良好 的 效果 ， 证 明 本 文 方法 能 
够 实现 多 AUV 在 未 知 环境 下 的 协同 避 障 ， 兼 顾 避 障 过 程 中 
的 编队 需求 ， 在 避 障 结束 后 能 够 快速 恢复 编队 ， 并 具有 可 扩 
展 性 。 未 来 的 工作 重点 是 依托 实验 室 的 AUV 进行 实物 验证 。 
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